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Abstract —Simplified models of the dynamics such as the 
linear inverted pendulum model (LIPM) have proven to per¬ 
form well for biped walking on flat ground. However, for more 
complex tasks the assumptions of these models can become 
limiting. For example, the LIPM does not allow for the control 
of contact forces independently, is limited to co-planar contacts 
and assumes that the angular momentum is zero. In this paper, 
we propose to use the full momentum equations of a humanoid 
robot in a trajectory optimization framework to plan its center 
of mass, linear and angular momentum trajectories. The model 
also allows for planning desired contact forces for each end- 
effector in arbitrary contact locations. We extend our previous 
results on LQR design for momentum control by computing 
the (linearized) optimal momentum feedback law in a receding 
horizon fashion. The resulting desired momentum and the 
associated feedback law are then used in a hierarchical whole 
body control approach. Simulation experiments show that the 
approach is computationally fast and is able to generate plans 
for locomotion on complex terrains while demonstrating good 
tracking performance for the full humanoid control. 

I. INTRODUCTION 

Humanoid robots locomoting and performing manipula¬ 
tion tasks on uneven ground are required to actively apply 
forces on objects and the floor in order to achieve their task 
successfully. A direct effect of contact forces is a change of 
momentum in the robot, which on the one hand is necessary 
to move the center of mass, but on the other hand restricts the 
type of limb motion that the robot can perform. The nonlinear 
nature of the angular momentum dynamics makes preview- 
based control computationally hard in general and even with 
an admissible angular momentum trajectory open control 
parameters like the joint motion and feedback control remain 
problematic for use in a whole body control framework. 

Successful applications of simplified momentum models 
have been shown on robots walking quite robustly over flat 
ground. A common approach is to use the linear inverted pen¬ 
dulum model (LIPM), which has been exploited for preview 
control since it was introduced by Kajita et. al [1]. In [2], a 
model predictive control (MPC) approach is formulated that 
finds foot steps on a flat ground together with a compatible 
CoM location on a horizontal plane. CoM profiles can then 
be realized on the full body together with other limb motion 
(e.g. swing leg) controllers [3], [4] [5], [6]. The assumption 
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of a horizontal CoM motion and flat ground can be relaxed 
to a pre-designed CoM height profile [7], [8]. Although, 
less restrictive than the original LIPM, these approaches are 
either built for point feet or leave parts of the dynamics 
uncontrolled. Depending on the terrain, pre-defining the CoM 
along a fixed direction may result in suboptimal or infeasible 
reaction forces. Approaches that leave the regime of linear 
models have been proposed, for instance for long jumps [9] 
leading to more complex task behavior. However, they often 
require task specific models that for example take into 
account the swing leg dynamics. Going even further, we have 
seen work that optimizes over the whole joint trajectories 
and the full momentum together [10]. The impressive near- 
to physical motions, however, come at a high computa¬ 
tional cost. Time-local controllers, have shown that balancing 
performance of robots is improved, when overall angular 
momentum is damped out directly [11], [12]. Nevertheless, it 
has remained unclear how angular momentum profiles should 
be chosen in non-static configurations. 

In this paper, we consider the full momentum dynamics 
of the robot for generation of CoM and momentum tra¬ 
jectories and the according reaction force profiles at each 
contact. We formulate the problem as a continuous-time 
optimal control problem in a sequential form. A mode 
schedule is predefined together with end effector trajectories. 
A desired angular momentum trajectory, which is required 
for the limb motion, is generated from a simple inverse 
kinematics forward integration and realized with admissible 
contact forces in a least squares optimal sense. Since the 
kinematics information is used before the optimization over 
the dynamics, our optimization procedure is relatively fast 
(for example compared to [10]). In our previous work [12] 
we have proposed a way to compute control gains for the 
momentum task using a LQR design approach. We showed 
that it was able to significantly improve performance for 
balancing tasks on a real humanoid. However, this approach 
was only used for stabilization. In this paper, we extend 
the approach to receding horizon tracking control. We use 
the planned trajectories to generate feedback gains using a 
LQR design where we linearize the non-linear momentum 
dynamics around the optimized trajectory. In a simulated 
stepping task, our humanoid traverses a terrain with changing 
height and angled stepping stones successfully. A whole- 
body controller computes joint torques that realize feedback 
loops on momentum as well as the swing leg motion and 
respect additional balancing and hardware constraints and 
generates physically realizable contact forces. A good track¬ 
ing of overall momentum is achieved using our receding 
horizon LQR design. 




Fig. 1. A sketch of the reduced model used through out this paper. It shows 
the center of mass (CoM) r of the system and contacts with the environment. 
States and controls are color-labeled blue and orange respectively. Flat 
contacts that consist of a force, center of pressure and normal torque (here 
fi, ri, pi) can be modeled (e.g. a hand touching a wall) as well as point 
contacts like p 2 - There is no restriction on the pose of contacts nor the 
CoM. Momenta generated around the CoM are not necessarily zero. 

This paper is organized as follows. In Sec [II] we formulate 
an optimal control problem to obtain momentum trajectories 
of the robot. Resulting trajectories are controlled with a 
LQR feedback design as discussed in Sec [Til] which is 
then incorporated into a whole-body control approach as 
explained in Sec In Sec [V| we demonstrate our control 
framework on a simulated stepping task on rough terrain. We 
discuss our results in Sec [VT] and finish with a conclusion in 
Sec Em 

II. MOMENTUM TRAJECTORY OPTIMIZATION 


the type of contact, it will be necessary to express constraints 
on the forces. In the case of point feet, no torques r- L can 
be generated. Torques that are generated at the CoP of a flat 
foot are required to be normal to the foot surface. Further, if 
we assume stationary contact points, we need to restrict the 
wrench to remain in a friction cone. 

B. Optimal control formulation 

In the following we use the momentum dynamics of the 
robot to plan for admissible contact forces that generate 
desired (linear and angular) momentum trajectories. Our 
goal is to compute force and contact point trajectories 
f^(£), Pi(t) that satisfy the momentum dynamics in 
Eqs and contact constraints at all time. These trajecto¬ 
ries are also required to steer the momentum through desired 
states over a time horizon T. Given the initial state of the 
robot CoM and momentum r(0), 1(0), k( 0), we can integrate 
Eqs ([2][4]) to obtain 
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In this section, we describe how the robot momentum is 
planned together with admissible contact forces. We will first 
discuss the reduced dynamics model which will then be used 
to phrase an optimal control problem to generate CoM and 
momentum profiles. At the end of the section we describe 
how a desired angular momentum is chosen for optimization. 


A. Dynamic Model 

We reduce the state of a robot to its center of mass (CoM) 
r and overall momentum 


h = 



( 1 ) 


with linear 1 and angular n momenta. The state of the system 
is controlled by forces f^ and torques acting at points 
on the mechanical structure. The momentum dynamics as 
illustrated in Fig [T| can be written 
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where M is the mass of the robot. The contact points 
can for instance be point foot locations that are touching the 
ground or they can represent the position of a handle that the 
robot holds on to. Flat feet can be modeled by several contact 
points on the foot surface. Equivalently, we can represent the 
effective force at the center of pressure (CoP). Depending on 


Note that the states are expressed as (nested) functions of the 
reaction forces. They are constructed by integrating, sum¬ 
ming and applying the cross-product on f^(£), Ti(t), 

Given a naive idea of what the desired CoM r desif) and 
momentum h des{t) should be (e.g. coming from an initial 
kinematic plan), we want to find contact forces that minimize 
the error 
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where we compute the errors at ti G [0; T] and Wi are 
diagonal weighting matrices that allow trade-offs between 
the cost terms. 

Adaptive end effector location: Throughout the discussion 
in this paper we assume that the end effector moves between 
a series of predefined locations. However, this assumption 
can be relaxed without making the optimization problem 
harder. We simply substitute p^ = p* + p^, where p^ is 
the foot sole location that remains stationary throughout a 
contact phase and p^ is the (time-varying) center of pressure 
inside of the foot sole. In this reformulation both, p^ and p^, 
will be optimization variables and are chosen automatically. 
The support planes still have to be decided before-hand, e.g. 
using a dedicated acyclic contact planer [13], [14]. 








C. Optimization procedure 

We will now formulate the described problem into an opti¬ 
mization problem. First, the reaction forces are formulated as 
weighted basis functions, more specifically as polynomials 
of the form 


N -1 

f(t;w) = a(t) ^ w k t k = § T (t) w, 
k=0 
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with weights Wi and basis functions t k summarized in 
vectors w. We define the mode-scheduling variable 
a(t) which specifies contact activation and deactivation and 
is set before-hand, for example it could be obtained by 
a higher level planner. Polynomials have the advantage of 
generating smooth force trajectories by construction. The 
forces can then be written as 
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where the subscript identifies the end effector, the super¬ 
script represents the coordinates and n is the normal vector of 
the foot sole. Contact forces, torques and CoPs thus become 
linear functions of polynomial coefficients. Expressing the 
states in Eqs (5][8) with Eqs (TTJ13]) substituted, gives us the 
states as functions of time and polynomial coefficients. At 
the core of operations required to carry out the result are 
multiplication, summation and integration of polynomials, 
which can be computed analytically. As a result, we phrased 
our optimal control problem in sequential form, where our 
controls evaluate directly to states and the dynamics equa¬ 
tions (cf. Eqs ([2][3])) are implicitly incorporated. This allows 
us to phrase our optimization problem 


min. J (x) (14) 

X 

s.t. |p J (t i ;x)| <p, j = x,y (15) 

< t, (16) 

0 < -|/ J (**;x)| + nf z (U-,-x.) < f z , j = x,y (17) 
ti = t 0 ...T 

where x is a concatenation of variables wj , v* , uj . We 
try to find polynomial coefficients x that minimize the error 
cost J while satisfying friction and support bound constraints 
on forces. Eqs bound the CoPs and torques. In 

Eq (17] ) we impose a friction cone constraint approximated as 
pyramid and upper bound it by a sufficiently large value f z to 
keep the polynomials from penetrating the lower bound and 
escaping above. In our constraints in Eqs ( p~5p7] ), we wish to 
have bounds that hold at all t G [0,T]; in practice however, 
we express them at a finite number of time steps. Given the 


limited flexibility of polynomials, we get minor penetration 
of those constraints in intervals (U;ti+ 1 ). Note that all our 
constraints are linear, whereas the objective function has 
quadratic terms as well as higher order (non-convex) terms 
due to the costs on angular momentum. Since the dynamics 
are not further simplified, we have the benefit that all states 
and controls are included in the cost J and none of them are 
left uncontrolled. 

Receding Horizon: Given that the problem in Eq (l4| ) is 
non-convex, we cannot expect to find a global optimum in 
general but need to find a local optimum starting from an 
initial guess x 0 . In our approach we use a receding horizon 
technique. We start out by solving our problem for a horizon 
T < T and obtain an optimal solution x* Q ^ . Typically, 
we start our desired motion such that it is easy to solve 
in the interval [0,T], i.e. the robot is standing still and 
applying gravity compensation is already optimal. Then, we 
formulate our problem for the interval [A, T + A] where we 
initialize our optimizer with the previous solution. This has 
the advantage that an initial solution can be bootstrapped 
from an initial easier-to-solve configuration and then moved 
over the horizon to the difficult parts of the motion. Further, 
as we push our implementation to run in real-time, we seek 
to use it in a receding horizon control setting. 

Desired Angular Momentum: Most simplified momentum 
models assume that angular momentum is desired to remain 
at zero. This, however, ignores the fact that momentum might 
be required to move the robot limbs. E.g. in a stepping task 
we need to swing a leg, which in turn generates a momentum 
around the hip. Thus, it is not trivial to decide what the 
desired angular momentum should look like. In order to 
overcome this issue, we integrate forward our desired swing 
leg trajectories using inverse kinematics. From the resulting 
joint motion we then compute r des(t), Kdes(t) (explained 
e.g. in [15]). Although dynamically not feasible, this method 
generates angular momentum profiles required to perform the 
task-imposed motion, e.g. swinging a leg. We iterate between 
kinematic forward integration of an optimized r (t) and 
optimizing a desired ndesit) with dynamic constraints (by 
solving the problem in Eqs ( [T4|[T7] )). In our experiments two 
passes already lead to convergence, i.e. forward integrated 
and optimized trajectories do not differ significantly. With 
this iteration we not only generate admissible force profiles 
that obey the momentum dynamics equations but also we 
can bootstrap angular momentum trajectories which are not 
obvious to design otherwise. Note that neither CoM nor 
momentum are predefined in advance. Instead an initial guess 
is given and the final trajectories are found automatically. 

III. MOMENTUM LQR 

In the previous section we described how reference tra¬ 
jectories in accordance with the momentum dynamics are 
obtained. In order to track those trajectories on the full robot, 
we propose a feedback law using a LQR design, which has 
shown superior performance compared to a naive PD gain 
approach in experiments on the real robot [12]. From our 
trajectory optimizer we obtain admissible states and controls 












y 


* 


r 


,A* = 


n 



(18) 


/(y*,A*) 


J_1 

M 1 


Mg + £f) 

E 1 "* + E(Pi - r) x ii 


(19) 


where we transform wrenches f 7 . t l to the stationary poles 
P %• 

The dynamics function in Eq (\9\ is discretized and lin¬ 
earized around the desired trajectories x*, A*. The resulting 
time varying-linear dynamics are then used to formalize a 
finite horizon LQR problem. This yields a control policy 


A = A* - K t (x - x*) (20) 


with time-varying feedforward and feedback terms that map 
errors in states into contact wrenches. Controlling the mo¬ 
mentum with this feedback law requires 6 DoF for wrenches 
at each contact. Since our whole-body controller incorporates 
additional control objectives and force constraints, we com¬ 
pute directly the momentum rate that the wrenches generate 


K t (x*-x) + h*, (21) 

where [.] x turns a cross-product into a matrix multiplica¬ 
tion. The resolution of momentum rate to contact wrenches 
is then left to our whole-body controller as described in the 
next section. In this LQR design a quadratic performance 
cost is set once and then optimal gains are computed at 
each time step for the corresponding contact configuration. 
As we discussed in our previous work, in order to achieve 
compatible results with diagonal PD gain matrices, we had 
to design gains for different contact configurations, whereas 
the LQR design requires one performance cost and generated 
suitable gains automatically. In contrast to our previous work 
we linearize around desired trajectories, whereas in our robot 
experiments we used only two key configurations; this may 
become limiting in more versatile tasks such as walking over 
a rough terrain. 



IV. WHOLE-BODY CONTROL 

The trajectories generated with the model in Eqs ffl 
define the CoM, momentum and end effector forces of our 
humanoid. In order to track these trajectories on the full 
robot, we need to generate joint torques accordingly and 
at the same time control the limb motion and guarantee 
that other constraints are obeyed, e.g. joint limits. For this 
time-local control problem, we use inverse dynamics in QP 
Cascades, which we applied successfully on the real robot in 
previous work [6]. It allows us to phrase feedback controllers 
and constraints as functions of joint accelerations q, external 
generalized forces A and joint torques r. For instance, we 
can express a cartesian controller on the swing foot as 



Fig. 2. The humanoid robot traversing a terrain with stepping stones of 
different hight and orientation. 


an affine function of joint accelerations or a momentum 
controller as a function of reaction forces. The latter, for 
example, would be 
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A reference momentum rate h re f can be chosen from a 
LQR design as demonstrated in our previous work [12] 
and extended in Sec [Till Given a set of controllers and 
tasks, we find torques that satisfy the dynamics equations 
of the full robot and at the same time generate the desired 
task feedback as good as possible. Tasks, however, may 
conflict, for instance moving the CoM may require moving 
the swing leg and vice versa. In these cases, QP cascade 
allow for two types of trade-offs; we can either weigh 
tasks against each other or we can prioritize them strictly. 
Especially when it comes to trade-offs between constraints, 
tasks of interest and redundancy resolution, prioritization 
can facilitate successful control design. 


V. SIMULATION RESULTS 

This section describes our simulation results of the pro¬ 
posed control framework. We use a model of our Sarcos 
humanoid in the SL simulation environment. Contacts are 
simulated with a penalty method and stiff springs. All 
experiments are performed on a 2.7 GHz intel i7 processor 
with 16gb ram. A task is generated where the robot is to 
walk on stepping stones that increase in height from one 
step to the other as visualized in Fig [2] and shown in the 
attached videcQ The z-axis of the inertial frame points up 
and the robot walks along the y-axis to the front. Two out 
of the four steps are tilted by 25°. Both the change in CoM 
height as well as the angled supports break the assumptions 
made in LIPM models and require the consideration of two 
separate contact wrenches. We generate swing leg trajectories 
using cubic splines to parameterize the pose of the foot. The 
humanoid starts out in double support at rest. After the first 3 
seconds the left foot breaks contact and moves to an angled 
support surface located to the front left-hand side of the robot 
at an increased height (as shown in Fig [2]). Then a contact 
switch occurs at every second, changing from single support 
to double support or vice versa. The second step is again a 

Uhe video is also available on www-amd. is . tuebingen .mpg. de/ 
~herzog/15_07-Humanoids.mp4 
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Rank 

Nr. of eq/ineq con¬ 
straints 

Constraint/Task 

1 

6 eq 

Newton Euler Equation 

2 

2 x 6 eq 

Contact constraints 


2x4 ineq 

Center of Presure 


2x4 ineq 

Friction cone 


2 x 14 ineq 

joint acceleration limits 

3 

6 eq 

LQR momentum control 


6 eq 

Cartesian swing foot control 


14 + 6 eq 

PD control on posture 

4 

2 x 6 eq 

Contact forces control 

5 

3 eq 

Base link orientation 


TABLE I 

Hierarchy of tasks in the stepping experiment 


support surface angled inwards to the robot and located to 
the front right-hand side. Finally, the robot takes two steps 
onto a horizontal plateau located slightly below knee height. 
The planner is initialized with a naive idea of the robot mo¬ 
tion where we simply keep the base at a certain height above 
the feet. After integrating the desired swing foot positions 
together with the base height using inverse kinematics (as 
described in Sec II-C), we obtain resulting r desif)^desif) 
trajectories that take into account the angular momentum 
required to swing the legs from one stepping stone to the 
other. The inverse kinematics solution, which is physically 
not consistent, is then adjusted in the trajectory optimization 
step to be admissible with respect to constrained forces and 
CoPs. None of the force constraints were notably violated. As 
can be seen in Fig [3] the inverse kinematics-generated CoM 
trajectory is modified significantly in the lateral direction 
and as a consequence the angular momentum in the y- 
direction is modified as well. After a second iteration of 
inverse kinematics integration and trajectory optimization, 
the results have sufficiently converged and we stop. Poly¬ 
nomials of order 3 are chosen and initialized with zero. The 
planning process took 4 min and converged after only two 
iterations. This allows us to generate a complex motion rather 
quickly compared to motion planners that are based on more 
extensive models of the robot. The numerical optimization 
problem in Eq[l4]is solved with SNOPT [16], a Sequential 
Quadratic Programming method. 

Next, we construct a hierarchy of feedback controllers and 
constraints in order to realize the momentum profile on the 
full humanoid. At the highest priority we express the phys¬ 
ical model of the full robot to obtain physically-consistent 
torques. This is followed by force and joint limits together 
with contact constraints. In the third priority we control the 
swing leg motion and the momentum and add a posture 
PD control with a relatively-low weight. In the priorities 
below, we regularize end effector forces and stabilize the base 
orientation. A summary of the task setup is given in Tab [T] 
The momentum is controlled with feedback gains designed 
as described in Sec [HI] where our performance cost is fixed 
for the whole run. We penalize state errors by 10, forces 
by 0.1 and torques by 0.5. Gains are generated over a 2sec 
horizon with a granularity of 200 time steps. A sequence of 
gains is recomputed every 10ms. 




Fig. 3. Plans for the CoM and horizontal angular momentum. IK 1, IK 2 
show the profiles obtained from the inverse kinematics passes, whereas the 
blue graph is the final optimized trajectory. The resulting lateral CoM (top 
plot) was adapted quite significantly over the planning process to make it 
physically compatible with the reaction forces. Angular momenta are found 
that allow for stepping motions required to traverse the terrain. 


The robot was able to traverse the terrain successfully. The 
momentum trajectories, which were both dynamically con¬ 
sistent as well as compatible with the robots limb motions, 
could be tracked well as shown in Figs ([6]-[5|. Torques were 
generated that are in the bounds of the physical robot’s torque 
limits. In the beginning of the task the planned CoM height 
forces the knees to stretch, which prevents the robot from 
using its knees to lift the CoM further, but instead it lifts 
the arms up rapidly. This can be avoided by adding a box 
constraint on the CoM in the optimization problem in order 
to consider kinematic limitations. 

The LQR gain matrices have non-zero off-diagonal values as 
expected (cf. Figg). For instance, we can see that angular 
momentum is generated in order to correct for errors in CoM 
and linear Momentum, which would not be possible with 
diagonal PD gains. In fact, we tried to track the planned 
motion using diagonal PD gains. We started with values 
similar to the diagonal of the LQR gain, but we could not 
find parameters that were stable throughout several contact 
situations as was the case for the LQR gains. Increasing the 
terrain difficulty was mainly problematic due to kinematic 
limits because our naive swing leg trajectories required to 
keep the heel on the ground during the whole support phase, 
thus limiting the stepping height. Lurther, we noted problems 
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Fig. 4. Here we show a heat map visualization of (a) a momentum gain 
and (b) a force gain. The top 6 rows in (b) correspond to the wrench of the 
right foot, whereas the bottom rows correspond to the left foot. As we can 
see, the gains contain off-diagonal terms leading to coupling terms between 
linear and angular momentum. These terms are ignored in a naive diagonal 
PD gain design. The bottom plot shows the norm of 3x3 sub-blocks of 
the momentum gain plotted over time. The vertical dashed lines at t = 3 
and t = 4 indicate contact switches. Gain profiles change significantly 
over time and contact configurations. The gains and momentum dynamics 
are discontinuous at contact switches. Nevertheless, discontinuities in joint 
torques were negligible. 



Fig. 5. Linear and angular momentum are tracked well as the robot walks 
over the terrain. The oscillations in the angular momentum at t=2 come from 
rapid arm motions when the robot was trying to move the CoM up and the 
knees were stretched. This can be avoided, e.g. by adding box constraints 
on the CoM in the trajectory optimization step to account for kinematic 
limits. 



with the simulator’s contact model, which caused sporadic 
spikes in the force profile when we applied strong forces on 
the ground. 

Overall, a complex task could be planned quickly including 
a non-trivial angular momentum profile that respected the 
end effector motion. The proposed feedback control law 
on the momentum showed good performance when it was 
embedded in an inverse dynamics task hierarchy. 

VI. DISCUSSION 

Relation to simplified Momentum Dynamics: The momen¬ 
tum model in Eqs ([2][4]) is often simplified further in order to 
obtain linear dynamics, which then leads to computationally 
more efficient algorithms. However, turning multiplicative 
terms between variables in Eq 0 into linear terms requires 
potentially restrictive assumptions. E.g. in the LIPM the CoM 
height r z is assumed to be constant, there is one effective p^, 
which lies in one horizontal plane, and the force is required 
to act along r — p. 

Since a constant CoM height may be limiting for tasks 
that require vertical movement, the authors in [8] allow a 
predefined (not constant) r z (t). Substituting this assumption 
into Eq ([4]), and assuming constant p^, turns the horizontal 
angular momentum into a linear function of forces and 


Fig. 6. Measured and desired CoM of the robot when it was traversing 
the terrain. As the plots show, good tracking performance can be achieved 
with the proposed LQR design on the momentum. 

torques. Assuming in addition that the nonlinear vertical an¬ 
gular momentum can be neglected, the authors end up again 
with linear dynamics. If we restrict our dynamics model 
further into a LIPM and use discretized dynamics (instead of 
polynomial trajectories) we can recover the approach of [2]. 
However, this can be restrictive if we want to step on various 
slopes or when a non-zero angular momentum is required, 
e.g. to consider limb motion. 

Computation time: In our current work, we focused on 
the capabilities of the multi-contact dynamics model to 
generate dynamic motions on uneven terrain and we showed 
that these behaviors can be controlled on a full humanoid 
robot. However, the goal of separating the control process 
into a predictive control generation on a lower dimensional 
model and time-local control on the full dynamics has the 
potential for a fast implementation in a MPC fashion. In our 
experiments we saw potential drawbacks in our numerical 
optimization in Eq ( p~4| ). Increasing the order of polynomials 
leads to slower convergence rates, whereas reducing the 
































































polynomial dimensions too far may lead to poor flexibility 
of the trajectory representation. This may be explained by 
the discrepancy of basis functions evaluated close to 0 and 
close to T leading to poorly conditioned problem. Piecewise 
constant trajectories may overcome this problem. Another 
point for improvement of the numerical procedure is the 
formulation of the optimal control problem. In the current 
formulation, the objective function in Eq ^ has terms up to 
4th order (squared norm of cross products). Our numerical 
solver is based on approximations up to second order, which 
may limit the region in which approximations are valid. It 
is possible (but out of the scope of this paper) to rewrite 
the problem into a (non-convex) Quadratically Constrained 
Quadratic Program where the objective function as well as 
the constraints are quadratic, leading to a better approxi¬ 
mation in second order methods and potentially improving 
convergence. Pushing the implementation towards an online 
control algorithm is part of our future work. 

VII. CONCLUSIONS 

We presented an approach to control contact forces and 
momentum for humanoid robots. CoM and momentum pro¬ 
files were obtained in an optimal control framework together 
with admissible contact forces. Feedback gains are generated 
from a LQR design, which generates time and contact- 
configuration dependent gains from a single performance 
cost. The resulting controller is embedded in an inverse 
dynamics-based whole-body controller together with other 
limb controllers and constraints. We demonstrated the control 
framework on a simulation of the Sarcos humanoid traversing 
rough terrain. Physically admissible momentum and force 
trajectories could be found relatively quickly and were 
tracked well during the task execution. In future work, we 
will implement the discussed speed-up and push the optimal 
controller to run online. 
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